//OpenCV2标准头文件

#include <iostream>
#include <time.h>
#include <ros/ros.h> 
#include <fstream>  //文件流库函数
#include <iomanip>
#include <sensor_msgs/NavSatFix.h>


using namespace std;

ofstream outfile1_fix;   //输出流
ofstream outfile2_fix;   //输出流
ofstream outfile3_fix;   //输出流

double a_bit = 6378137.0;
double b_bit = 6356752.31414;
double e2_bit = (pow((a_bit*a_bit-b_bit*b_bit),0.5)/a_bit)*(pow((a_bit*a_bit-b_bit*b_bit),0.5)/a_bit);
double PI_bit = 3.1415926;
// double start_point_lat_rad = 0.697389;
// double start_point_lon_rad = 2.02997;

void gps_xyz(double lat2, double lon2, double *x_east, double *y_north, double start_point_lat_rad, double start_point_lon_rad)
{

    double lat2_rad = lat2 * PI_bit /180.0;
    double lon2_rad = lon2 * PI_bit /180.0;
    // cout << "lat2_rad" << lat2_rad << "  " << lon2_rad << endl;

    double dB = lat2_rad - start_point_lat_rad;
    double dL = lon2_rad - start_point_lon_rad;
    double Bm = (start_point_lat_rad + lat2_rad) /2;

    double mm = a_bit * (1-e2_bit)*pow(1-e2_bit*sin(Bm)*sin(Bm), -1.5);
    double nm = a_bit * pow(1-e2_bit*sin(Bm)*sin(Bm), -0.5);

    double dZ = cos(Bm) *nm *dL;  	//指向正东
    double dX = mm *dB;          		//指向正北

    *x_east = dZ;
    *y_north = dX;
    // cout << "x_east y_north  " << dZ << "  " << dX << endl;
}


void sub1_Fix_cb(const sensor_msgs::NavSatFix& msg)
{
    static int cnt = 1;
    double x_east,y_north;
    static double start_point_lat , start_point_lon;
    if(cnt <= 10)
    {
        start_point_lat += msg.latitude * PI_bit /180.0;
        start_point_lon += msg.longitude * PI_bit /180.0;
        if(cnt == 10)
        {
            start_point_lat = start_point_lat / 10.0;
            start_point_lon = start_point_lon / 10.0;
            cout << "start_point_lat start_point_lon  " << start_point_lat << "  " << start_point_lon << endl;
        }
        cnt++;
    }
    else
    {
        gps_xyz(msg.latitude, msg.longitude,&x_east,&y_north, start_point_lat, start_point_lon);
        outfile1_fix << setiosflags(ios::fixed)<< setprecision(7) << x_east << "\t" << y_north << endl;
        cout << "x_east y_north  " << x_east << "  " << y_north << endl;
    }    

    
}
void sub2_Fix_cb(const sensor_msgs::NavSatFix& msg)
{
    // double x_east,y_north;
    // gps_xyz(msg.latitude,msg.longitude,&x_east,&y_north);
    // outfile2_fix << setiosflags(ios::fixed)<< setprecision(7) << x_east << "\t" << y_north << endl;
}
void sub3_Fix_cb(const sensor_msgs::NavSatFix& msg)
{
    // double x_east,y_north;
    // gps_xyz(msg.latitude,msg.longitude,&x_east,&y_north);
    // outfile3_fix << setiosflags(ios::fixed)<< setprecision(7) << x_east << "\t" << y_north << endl;
}


int main(int argc, char *argv[])
{
    //视频保存位置
    string saveFilePath = "/home/linux/work/dataset/saveFile/";  

    ros::init(argc, argv, "save_file_node");
    ros::NodeHandle nh;//创建句柄
    ros::Subscriber sub1_Fix,sub2_Fix,sub3_Fix;

    string uavname1 = "bebop";
    string uavname2 = "bebop2";
    string uavname3 = "bebop4";

    //设置订阅主题 
    sub1_Fix = nh.subscribe(uavname1 + "/fix",1,sub1_Fix_cb);
    sub2_Fix = nh.subscribe(uavname2 + "/fix",1,sub2_Fix_cb);
    sub3_Fix = nh.subscribe(uavname3 + "/fix",1,sub3_Fix_cb);
    ros::Rate loop_rate(30);

    // 获取当前时间，精确到秒
    time_t currentTime = std::time(NULL);
    char chCurrentTime[64];
    std::strftime(chCurrentTime, sizeof(chCurrentTime), "%Y-%m-%d-%H-%M-%S", std::localtime(&currentTime)); //年月日 时分秒
    std::string stCurrentTime = chCurrentTime;// 转为string
    
    // 文件

    outfile1_fix.open(saveFilePath + uavname1 + "-"  + stCurrentTime+ ".txt" , ios::trunc);
    // outfile2_fix.open(saveFilePath + uavname2 + "-"  + stCurrentTime+ ".txt" , ios::trunc);
    // outfile3_fix.open(saveFilePath + uavname3 + "-"  + stCurrentTime+ ".txt" , ios::trunc);


    if(!outfile1_fix.is_open() || !outfile1_fix.is_open() || !outfile1_fix.is_open())
    {
        cout << "fail to open!" << endl;
        return -1;
    }


    while(ros::ok()) 
    {

        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}
